/**
* \brief 
* \author pengcheng (pengcheng@yslrpch@126.com)
* \date 2020-04-26
* \attention CopyrightÃ‚Â©ADC Technology(tianjin)Co.Ltd
* \attention Refer to COPYRIGHT.txt for complete terms of copyright notice
*/


#include <stereo_vision/sgm.h>
#include "stereo_vision/internal.h"
#include "stereo_vision/impl/utility.hpp"

namespace {
	template<typename SRC_T, typename DST_T>
	__global__ void check_consistency_kernel(DST_T* d_leftDisp, const DST_T* d_rightDisp, const SRC_T* d_left, int width, int height, int src_pitch, int dst_pitch, bool subpixel, int LR_max_diff) {

		const int j = blockIdx.x * blockDim.x + threadIdx.x;
		const int i = blockIdx.y * blockDim.y + threadIdx.y;

		// left-right consistency check, only on leftDisp, but could be done for rightDisp too

		SRC_T mask = d_left[i * src_pitch + j];
		DST_T org = d_leftDisp[i * dst_pitch + j];
		int d = org;
		if (subpixel) {
			d >>= adc::StereoSGM::SUBPIXEL_SHIFT;
		}
		int k = j - d;
		if (mask == 0 || org == adc::INVALID_DISP || (k >= 0 && k < width && LR_max_diff >= 0 && abs(d_rightDisp[i * dst_pitch + k] - d) > LR_max_diff)) {
			// masked or left-right inconsistent pixel -> invalid
			d_leftDisp[i * dst_pitch + j] = static_cast<DST_T>(adc::INVALID_DISP);
		}
	}
}

namespace adc {
	namespace details {

		void check_consistency(uint8_t* d_left_disp, const uint8_t* d_right_disp, const void* d_src_left, int width, int height, int depth_bits, int src_pitch, int dst_pitch, bool subpixel, int LR_max_diff) {

			const dim3 blocks(width / 16, height / 16);
			const dim3 threads(16, 16);
			if (depth_bits == 16) {
				check_consistency_kernel<uint16_t><<<blocks, threads>>>(d_left_disp, d_right_disp, (uint16_t*)d_src_left, width, height, src_pitch, dst_pitch, subpixel, LR_max_diff);
			}
			else if (depth_bits == 8) {
				check_consistency_kernel<uint8_t><<<blocks, threads>>>(d_left_disp, d_right_disp, (uint8_t*)d_src_left, width, height, src_pitch, dst_pitch, subpixel, LR_max_diff);
			}

			CudaKernelCheck();
		}

		void check_consistency(uint16_t* d_left_disp, const uint16_t* d_right_disp, const void* d_src_left, int width, int height, int depth_bits, int src_pitch, int dst_pitch, bool subpixel, int LR_max_diff) {

			const dim3 blocks(width / 16, height / 16);
			const dim3 threads(16, 16);
			if (depth_bits == 16) {
				check_consistency_kernel<uint16_t><<<blocks, threads>>>(d_left_disp, d_right_disp, (uint16_t*)d_src_left, width, height, src_pitch, dst_pitch, subpixel, LR_max_diff);
			}
			else if (depth_bits == 8) {
				check_consistency_kernel<uint8_t><<<blocks, threads>>>(d_left_disp, d_right_disp, (uint8_t*)d_src_left, width, height, src_pitch, dst_pitch, subpixel, LR_max_diff);
			}
			
			CudaKernelCheck();	
		}

	}
}
